/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
 * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
 * Copyright 2011-2016  Jose Luis Blanco (joseluisblancoc@gmail.com).
 *   All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

/** \mainpage nanoflann C++ API documentation
 *  nanoflann is a C++ header-only library for building KD-Trees, mostly
 *  optimized for 2D or 3D point clouds.
 *
 *  nanoflann does not require compiling or installing, just an
 *  #include <nanoflann.hpp> in your code.
 *
 *  See:
 *   - <a href="modules.html" >C++ API organized by modules</a>
 *   - <a href="https://github.com/jlblancoc/nanoflann" >Online README</a>
 *   - <a href="http://jlblancoc.github.io/nanoflann/" >Doxygen
 * documentation</a>
 */

#ifndef NANOFLANN_HPP_
#define NANOFLANN_HPP_

#include <algorithm>
#include <array>
#include <cassert>
#include <cmath>   // for abs()
#include <cstdio>  // for fwrite()
#include <cstdlib> // for abs()
#include <functional>
#include <limits> // std::reference_wrapper
#include <stdexcept>
#include <vector>

/** Library version: 0xMmP (M=Major,m=minor,P=patch) */
#define NANOFLANN_VERSION 0x132

// Avoid conflicting declaration of min/max macros in windows headers
#if !defined(NOMINMAX) && \
    (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
#define NOMINMAX
#ifdef max
#undef max
#undef min
#endif
#endif

namespace nanoflann {
/** @addtogroup nanoflann_grp nanoflann C++ library for ANN
 *  @{ */

/** the PI constant (required to avoid MSVC missing symbols) */
    template<typename T>
    T pi_const() {
        return static_cast<T>(3.14159265358979323846);
    }

/**
 * Traits if object is resizable and assignable (typically has a resize | assign
 * method)
 */
    template<typename T, typename = int>
    struct has_resize : std::false_type {
    };

    template<typename T>
    struct has_resize<T, decltype((void) std::declval<T>().resize(1), 0)>
            : std::true_type {
    };

    template<typename T, typename = int>
    struct has_assign : std::false_type {
    };

    template<typename T>
    struct has_assign<T, decltype((void) std::declval<T>().assign(1, 0), 0)>
            : std::true_type {
    };

/**
 * Free function to resize a resizable object
 */
    template<typename Container>
    inline typename std::enable_if<has_resize<Container>::value, void>::type
    resize(Container &c, const size_t nElements) {
        c.resize(nElements);
    }

/**
 * Free function that has no effects on non resizable containers (e.g.
 * std::array) It raises an exception if the expected size does not match
 */
    template<typename Container>
    inline typename std::enable_if<!has_resize<Container>::value, void>::type
    resize(Container &c, const size_t nElements) {
        if (nElements != c.size())
            throw std::logic_error("Try to change the size of a std::array.");
    }

/**
 * Free function to assign to a container
 */
    template<typename Container, typename T>
    inline typename std::enable_if<has_assign<Container>::value, void>::type
    assign(Container &c, const size_t nElements, const T &value) {
        c.assign(nElements, value);
    }

/**
 * Free function to assign to a std::array
 */
    template<typename Container, typename T>
    inline typename std::enable_if<!has_assign<Container>::value, void>::type
    assign(Container &c, const size_t nElements, const T &value) {
        for (size_t i = 0; i < nElements; i++)
            c[i] = value;
    }

/** @addtogroup result_sets_grp Result set classes
 *  @{ */
    template<typename _DistanceType, typename _IndexType = size_t,
            typename _CountType = size_t>
    class KNNResultSet {
    public:
        typedef _DistanceType DistanceType;
        typedef _IndexType IndexType;
        typedef _CountType CountType;

    private:
        IndexType *indices;
        DistanceType *dists;
        CountType capacity;
        CountType count;

    public:
        inline KNNResultSet(CountType capacity_)
                : indices(0), dists(0), capacity(capacity_), count(0) {}

        inline void init(IndexType *indices_, DistanceType *dists_) {
            indices = indices_;
            dists = dists_;
            count = 0;
            if (capacity)
                dists[capacity - 1] = (std::numeric_limits<DistanceType>::max)();
        }

        inline CountType size() const { return count; }

        inline bool full() const { return count == capacity; }

        /**
         * Called during search to add an element matching the criteria.
         * @return true if the search should be continued, false if the results are
         * sufficient
         */
        inline bool addPoint(DistanceType dist, IndexType index) {
            CountType i;
            for (i = count; i > 0; --i) {
#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same
                // distance, the one with the lowest-index will be
                // returned first.
if ((dists[i - 1] > dist) ||
((dist == dists[i - 1]) && (indices[i - 1] > index))) {
#else
                if (dists[i - 1] > dist) {
#endif
                    if (i < capacity) {
                        dists[i] = dists[i - 1];
                        indices[i] = indices[i - 1];
                    }
                } else
                    break;
            }
            if (i < capacity) {
                dists[i] = dist;
                indices[i] = index;
            }
            if (count < capacity)
                count++;

            // tell caller that the search shall continue
            return true;
        }

        inline DistanceType worstDist() const { return dists[capacity - 1]; }
    };

/** operator "<" for std::sort() */
    struct IndexDist_Sorter {
        /** PairType will be typically: std::pair<IndexType,DistanceType> */
        template<typename PairType>
        inline bool operator()(const PairType &p1, const PairType &p2) const {
            return p1.second < p2.second;
        }
    };

/**
 * A result-set class used when performing a radius based search.
 */
    template<typename _DistanceType, typename _IndexType = size_t>
    class RadiusResultSet {
    public:
        typedef _DistanceType DistanceType;
        typedef _IndexType IndexType;

    public:
        const DistanceType radius;

        std::vector<std::pair<IndexType, DistanceType>> &m_indices_dists;

        inline RadiusResultSet(
                DistanceType radius_,
                std::vector<std::pair<IndexType, DistanceType>> &indices_dists)
                : radius(radius_), m_indices_dists(indices_dists) {
            init();
        }

        inline void init() { clear(); }

        inline void clear() { m_indices_dists.clear(); }

        inline size_t size() const { return m_indices_dists.size(); }

        inline bool full() const { return true; }

        /**
         * Called during search to add an element matching the criteria.
         * @return true if the search should be continued, false if the results are
         * sufficient
         */
        inline bool addPoint(DistanceType dist, IndexType index) {
            if (dist < radius)
                m_indices_dists.push_back(std::make_pair(index, dist));
            return true;
        }

        inline DistanceType worstDist() const { return radius; }

        /**
         * Find the worst result (furtherest neighbor) without copying or sorting
         * Pre-conditions: size() > 0
         */
        std::pair<IndexType, DistanceType> worst_item() const {
            if (m_indices_dists.empty())
                throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on "
                                         "an empty list of results.");
            typedef
            typename std::vector<std::pair<IndexType, DistanceType>>::const_iterator
                    DistIt;
            DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(),
                                         IndexDist_Sorter());
            return *it;
        }
    };

/** @} */

/** @addtogroup loadsave_grp Load/save auxiliary functions
 * @{ */
    template<typename T>
    void save_value(FILE *stream, const T &value, size_t count = 1) {
        fwrite(&value, sizeof(value), count, stream);
    }

    template<typename T>
    void save_value(FILE *stream, const std::vector<T> &value) {
        size_t size = value.size();
        fwrite(&size, sizeof(size_t), 1, stream);
        fwrite(&value[0], sizeof(T), size, stream);
    }

    template<typename T>
    void load_value(FILE *stream, T &value, size_t count = 1) {
        size_t read_cnt = fread(&value, sizeof(value), count, stream);
        if (read_cnt != count) {
            throw std::runtime_error("Cannot read from file");
        }
    }

    template<typename T>
    void load_value(FILE *stream, std::vector<T> &value) {
        size_t size;
        size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
        if (read_cnt != 1) {
            throw std::runtime_error("Cannot read from file");
        }
        value.resize(size);
        read_cnt = fread(&value[0], sizeof(T), size, stream);
        if (read_cnt != size) {
            throw std::runtime_error("Cannot read from file");
        }
    }
/** @} */

/** @addtogroup metric_grp Metric (distance) classes
 * @{ */

    struct Metric {
    };

/** Manhattan distance functor (generic version, optimized for
 * high-dimensionality data sets). Corresponding distance traits:
 * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float,
 * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
 * (e.g. float, double, int64_t)
 */
    template<class T, class DataSource, typename _DistanceType = T>
    struct L1_Adaptor {
        typedef T ElementType;
        typedef _DistanceType DistanceType;

        const DataSource &data_source;

        L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}

        inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
                                       DistanceType worst_dist = -1) const {
            DistanceType result = DistanceType();
            const T *last = a + size;
            const T *lastgroup = last - 3;
            size_t d = 0;

            /* Process 4 items with each loop for efficiency. */
            while (a < lastgroup) {
                const DistanceType diff0 =
                        std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
                const DistanceType diff1 =
                        std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
                const DistanceType diff2 =
                        std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
                const DistanceType diff3 =
                        std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
                result += diff0 + diff1 + diff2 + diff3;
                a += 4;
                if ((worst_dist > 0) && (result > worst_dist)) {
                    return result;
                }
            }
            /* Process last 0-3 components.  Not needed for standard vector lengths. */
            while (a < last) {
                result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
            }
            return result;
        }

        template<typename U, typename V>
        inline DistanceType accum_dist(const U a, const V b, const size_t) const {
            return std::abs(a - b);
        }
    };

/** Squared Euclidean distance functor (generic version, optimized for
 * high-dimensionality data sets). Corresponding distance traits:
 * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float,
 * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
 * (e.g. float, double, int64_t)
 */
    template<class T, class DataSource, typename _DistanceType = T>
    struct L2_Adaptor {
        typedef T ElementType;
        typedef _DistanceType DistanceType;

        const DataSource &data_source;

        L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}

        inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
                                       DistanceType worst_dist = -1) const {
            DistanceType result = DistanceType();
            const T *last = a + size;
            const T *lastgroup = last - 3;
            size_t d = 0;

            /* Process 4 items with each loop for efficiency. */
            while (a < lastgroup) {
                const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
                const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
                const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
                const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
                result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
                a += 4;
                if ((worst_dist > 0) && (result > worst_dist)) {
                    return result;
                }
            }
            /* Process last 0-3 components.  Not needed for standard vector lengths. */
            while (a < last) {
                const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
                result += diff0 * diff0;
            }
            return result;
        }

        template<typename U, typename V>
        inline DistanceType accum_dist(const U a, const V b, const size_t) const {
            return (a - b) * (a - b);
        }
    };

/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality
 * datasets, like 2D or 3D point clouds) Corresponding distance traits:
 * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double,
 * float, uint8_t) \tparam _DistanceType Type of distance variables (must be
 * signed) (e.g. float, double, int64_t)
 */
    template<class T, class DataSource, typename _DistanceType = T>
    struct L2_Simple_Adaptor {
        typedef T ElementType;
        typedef _DistanceType DistanceType;

        const DataSource &data_source;

        L2_Simple_Adaptor(const DataSource &_data_source)
                : data_source(_data_source) {}

        inline DistanceType evalMetric(const T *a, const size_t b_idx,
                                       size_t size) const {
            DistanceType result = DistanceType();
            for (size_t i = 0; i < size; ++i) {
                const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i);
                result += diff * diff;
            }
            return result;
        }

        template<typename U, typename V>
        inline DistanceType accum_dist(const U a, const V b, const size_t) const {
            return (a - b) * (a - b);
        }
    };

/** SO2 distance functor
 *  Corresponding distance traits: nanoflann::metric_SO2
 * \tparam T Type of the elements (e.g. double, float)
 * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
 * float, double) orientation is constrained to be in [-pi, pi]
 */
    template<class T, class DataSource, typename _DistanceType = T>
    struct SO2_Adaptor {
        typedef T ElementType;
        typedef _DistanceType DistanceType;

        const DataSource &data_source;

        SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}

        inline DistanceType evalMetric(const T *a, const size_t b_idx,
                                       size_t size) const {
            return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1),
                              size - 1);
        }

        /** Note: this assumes that input angles are already in the range [-pi,pi] */
        template<typename U, typename V>
        inline DistanceType accum_dist(const U a, const V b, const size_t) const {
            DistanceType result = DistanceType();
            DistanceType PI = pi_const<DistanceType>();
            result = b - a;
            if (result > PI)
                result -= 2 * PI;
            else if (result < -PI)
                result += 2 * PI;
            return result;
        }
    };

/** SO3 distance functor (Uses L2_Simple)
 *  Corresponding distance traits: nanoflann::metric_SO3
 * \tparam T Type of the elements (e.g. double, float)
 * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
 * float, double)
 */
    template<class T, class DataSource, typename _DistanceType = T>
    struct SO3_Adaptor {
        typedef T ElementType;
        typedef _DistanceType DistanceType;

        L2_Simple_Adaptor<T, DataSource> distance_L2_Simple;

        SO3_Adaptor(const DataSource &_data_source)
                : distance_L2_Simple(_data_source) {}

        inline DistanceType evalMetric(const T *a, const size_t b_idx,
                                       size_t size) const {
            return distance_L2_Simple.evalMetric(a, b_idx, size);
        }

        template<typename U, typename V>
        inline DistanceType accum_dist(const U a, const V b, const size_t idx) const {
            return distance_L2_Simple.accum_dist(a, b, idx);
        }
    };

/** Metaprogramming helper traits class for the L1 (Manhattan) metric */
    struct metric_L1 : public Metric {
        template<class T, class DataSource>
        struct traits {
            typedef L1_Adaptor<T, DataSource> distance_t;
        };
    };
/** Metaprogramming helper traits class for the L2 (Euclidean) metric */
    struct metric_L2 : public Metric {
        template<class T, class DataSource>
        struct traits {
            typedef L2_Adaptor<T, DataSource> distance_t;
        };
    };
/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
    struct metric_L2_Simple : public Metric {
        template<class T, class DataSource>
        struct traits {
            typedef L2_Simple_Adaptor<T, DataSource> distance_t;
        };
    };
/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
    struct metric_SO2 : public Metric {
        template<class T, class DataSource>
        struct traits {
            typedef SO2_Adaptor<T, DataSource> distance_t;
        };
    };
/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
    struct metric_SO3 : public Metric {
        template<class T, class DataSource>
        struct traits {
            typedef SO3_Adaptor<T, DataSource> distance_t;
        };
    };

/** @} */

/** @addtogroup param_grp Parameter structs
 * @{ */

/**  Parameters (see README.md) */
    struct KDTreeSingleIndexAdaptorParams {
        KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
                : leaf_max_size(_leaf_max_size) {}

        size_t leaf_max_size;
    };

/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
    struct SearchParams {
        /** Note: The first argument (checks_IGNORED_) is ignored, but kept for
         * compatibility with the FLANN interface */
        SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
                : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}

        int checks;  //!< Ignored parameter (Kept for compatibility with the FLANN
        //!< interface).
        float eps;   //!< search for eps-approximate neighbours (default: 0)
        bool sorted; //!< only for radius search, require neighbours sorted by
        //!< distance (default: true)
    };
/** @} */

/** @addtogroup memalloc_grp Memory allocation
 * @{ */

/**
 * Allocates (using C's malloc) a generic type T.
 *
 * Params:
 *     count = number of instances to allocate.
 * Returns: pointer (of type T*) to memory buffer
 */
    template<typename T>
    inline T *allocate(size_t count = 1) {
        T *mem = static_cast<T *>(::malloc(sizeof(T) * count));
        return mem;
    }

/**
 * Pooled storage allocator
 *
 * The following routines allow for the efficient allocation of storage in
 * small chunks from a specified pool.  Rather than allowing each structure
 * to be freed individually, an entire pool of storage is freed at once.
 * This method has two advantages over just using malloc() and free().  First,
 * it is far more efficient for allocating small objects, as there is
 * no overhead for remembering all the information needed to free each
 * object or consolidating fragmented memory.  Second, the decision about
 * how long to keep an object is made at the time of allocation, and there
 * is no need to track down all the objects to free them.
 *
 */

    const size_t WORDSIZE = 16;
    const size_t BLOCKSIZE = 8192;

    class PooledAllocator {
        /* We maintain memory alignment to word boundaries by requiring that all
            allocations be in multiples of the machine wordsize.  */
        /* Size of machine word in bytes.  Must be power of 2. */
        /* Minimum number of bytes requested at a time from	the system.  Must be
         * multiple of WORDSIZE. */

        size_t remaining; /* Number of bytes left in current block of storage. */
        void *base;       /* Pointer to base of current block of storage. */
        void *loc;        /* Current location in block to next allocate memory. */

        void internal_init() {
            remaining = 0;
            base = NULL;
            usedMemory = 0;
            wastedMemory = 0;
        }

    public:
        size_t usedMemory;
        size_t wastedMemory;

        /**
            Default constructor. Initializes a new pool.
         */
        PooledAllocator() { internal_init(); }

        /**
         * Destructor. Frees all the memory allocated in this pool.
         */
        ~PooledAllocator() { free_all(); }

        /** Frees all allocated memory chunks */
        void free_all() {
            while (base != NULL) {
                void *prev =
                        *(static_cast<void **>(base)); /* Get pointer to prev block. */
                ::free(base);
                base = prev;
            }
            internal_init();
        }

        /**
         * Returns a pointer to a piece of new memory of the given size in bytes
         * allocated from the pool.
         */
        void *malloc(const size_t req_size) {
            /* Round size up to a multiple of wordsize.  The following expression
                only works for WORDSIZE that is a power of 2, by masking last bits of
                incremented size to zero.
             */
            const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);

            /* Check whether a new block must be allocated.  Note that the first word
                of a block is reserved for a pointer to the previous block.
             */
            if (size > remaining) {

                wastedMemory += remaining;

                /* Allocate new storage. */
                const size_t blocksize =
                        (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE)
                        ? size + sizeof(void *) + (WORDSIZE - 1)
                        : BLOCKSIZE;

                // use the standard C malloc to allocate memory
                void *m = ::malloc(blocksize);
                if (!m) {
                    fprintf(stderr, "Failed to allocate memory.\n");
                    return NULL;
                }

                /* Fill first word of new block with pointer to previous block. */
                static_cast<void **>(m)[0] = base;
                base = m;

                size_t shift = 0;
                // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) &
                // (WORDSIZE-1))) & (WORDSIZE-1);

                remaining = blocksize - sizeof(void *) - shift;
                loc = (static_cast<char *>(m) + sizeof(void *) + shift);
            }
            void *rloc = loc;
            loc = static_cast<char *>(loc) + size;
            remaining -= size;

            usedMemory += size;

            return rloc;
        }

        /**
         * Allocates (using this pool) a generic type T.
         *
         * Params:
         *     count = number of instances to allocate.
         * Returns: pointer (of type T*) to memory buffer
         */
        template<typename T>
        T *allocate(const size_t count = 1) {
            T *mem = static_cast<T *>(this->malloc(sizeof(T) * count));
            return mem;
        }
    };
/** @} */

/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
 * @{ */

/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors
 * when DIM=-1. Fixed size version for a generic DIM:
 */
    template<int DIM, typename T>
    struct array_or_vector_selector {
        typedef std::array<T, DIM> container_t;
    };
/** Dynamic size version */
    template<typename T>
    struct array_or_vector_selector<-1, T> {
        typedef std::vector<T> container_t;
    };

/** @} */

/** kd-tree base-class
 *
 * Contains the member functions common to the classes KDTreeSingleIndexAdaptor
 * and KDTreeSingleIndexDynamicAdaptor_.
 *
 * \tparam Derived The name of the class which inherits this class.
 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
 * \tparam Distance The distance metric to use, these are all classes derived
 * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for
 * 3D points) \tparam IndexType Will be typically size_t or int
 */

    template<class Derived, typename Distance, class DatasetAdaptor, int DIM = -1,
            typename IndexType = size_t>
    class KDTreeBaseClass {

    public:
        /** Frees the previously-built index. Automatically called within
         * buildIndex(). */
        void freeIndex(Derived &obj) {
            obj.pool.free_all();
            obj.root_node = NULL;
            obj.m_size_at_index_build = 0;
        }

        typedef typename Distance::ElementType ElementType;
        typedef typename Distance::DistanceType DistanceType;

        /*--------------------- Internal Data Structures --------------------------*/
        struct Node {
            /** Union used because a node can be either a LEAF node or a non-leaf node,
             * so both data fields are never used simultaneously */
            union {
                struct leaf {
                    IndexType left, right; //!< Indices of points in leaf node
                } lr;
                struct nonleaf {
                    int divfeat;                  //!< Dimension used for subdivision.
                    DistanceType divlow, divhigh; //!< The values used for subdivision.
                } sub;
            } node_type;
            Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node)
        };

        typedef Node *NodePtr;

        struct Interval {
            ElementType low, high;
        };

        /**
         *  Array of indices to vectors in the dataset.
         */
        std::vector<IndexType> vind;

        NodePtr root_node;

        size_t m_leaf_max_size;

        size_t m_size;                //!< Number of current points in the dataset
        size_t m_size_at_index_build; //!< Number of points in the dataset when the
        //!< index was built
        int dim;                      //!< Dimensionality of each data point

        /** Define "BoundingBox" as a fixed-size or variable-size container depending
         * on "DIM" */
        typedef
        typename array_or_vector_selector<DIM, Interval>::container_t BoundingBox;

        /** Define "distance_vector_t" as a fixed-size or variable-size container
         * depending on "DIM" */
        typedef typename array_or_vector_selector<DIM, DistanceType>::container_t
                distance_vector_t;

        /** The KD-tree used to find neighbours */

        BoundingBox root_bbox;

        /**
         * Pooled memory allocator.
         *
         * Using a pooled memory allocator is more efficient
         * than allocating memory directly when there is a large
         * number small of memory allocations.
         */
        PooledAllocator pool;

        /** Returns number of points in dataset  */
        size_t size(const Derived &obj) const { return obj.m_size; }

        /** Returns the length of each point in the dataset */
        size_t veclen(const Derived &obj) {
            return static_cast<size_t>(DIM > 0 ? DIM : obj.dim);
        }

        /// Helper accessor to the dataset points:
        inline ElementType dataset_get(const Derived &obj, size_t idx,
                                       int component) const {
            return obj.dataset.kdtree_get_pt(idx, component);
        }

        /**
         * Computes the inde memory usage
         * Returns: memory used by the index
         */
        size_t usedMemory(Derived &obj) {
            return obj.pool.usedMemory + obj.pool.wastedMemory +
                   obj.dataset.kdtree_get_point_count() *
                   sizeof(IndexType); // pool memory and vind array memory
        }

        void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,
                           int element, ElementType &min_elem,
                           ElementType &max_elem) {
            min_elem = dataset_get(obj, ind[0], element);
            max_elem = dataset_get(obj, ind[0], element);
            for (IndexType i = 1; i < count; ++i) {
                ElementType val = dataset_get(obj, ind[i], element);
                if (val < min_elem)
                    min_elem = val;
                if (val > max_elem)
                    max_elem = val;
            }
        }

        /**
         * Create a tree node that subdivides the list of vecs from vind[first]
         * to vind[last].  The routine is called recursively on each sublist.
         *
         * @param left index of the first vector
         * @param right index of the last vector
         */
        NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right,
                           BoundingBox &bbox) {
            NodePtr node = obj.pool.template allocate<Node>(); // allocate memory

            /* If too few exemplars remain, then make this a leaf node. */
            if ((right - left) <= static_cast<IndexType>(obj.m_leaf_max_size)) {
                node->child1 = node->child2 = NULL; /* Mark as leaf node. */
                node->node_type.lr.left = left;
                node->node_type.lr.right = right;

                // compute bounding-box of leaf points
                for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
                    bbox[i].low = dataset_get(obj, obj.vind[left], i);
                    bbox[i].high = dataset_get(obj, obj.vind[left], i);
                }
                for (IndexType k = left + 1; k < right; ++k) {
                    for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
                        if (bbox[i].low > dataset_get(obj, obj.vind[k], i))
                            bbox[i].low = dataset_get(obj, obj.vind[k], i);
                        if (bbox[i].high < dataset_get(obj, obj.vind[k], i))
                            bbox[i].high = dataset_get(obj, obj.vind[k], i);
                    }
                }
            } else {
                IndexType idx;
                int cutfeat;
                DistanceType cutval;
                middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval,
                             bbox);

                node->node_type.sub.divfeat = cutfeat;

                BoundingBox left_bbox(bbox);
                left_bbox[cutfeat].high = cutval;
                node->child1 = divideTree(obj, left, left + idx, left_bbox);

                BoundingBox right_bbox(bbox);
                right_bbox[cutfeat].low = cutval;
                node->child2 = divideTree(obj, left + idx, right, right_bbox);

                node->node_type.sub.divlow = left_bbox[cutfeat].high;
                node->node_type.sub.divhigh = right_bbox[cutfeat].low;

                for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
                    bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
                    bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
                }
            }

            return node;
        }

        void middleSplit_(Derived &obj, IndexType *ind, IndexType count,
                          IndexType &index, int &cutfeat, DistanceType &cutval,
                          const BoundingBox &bbox) {
            const DistanceType EPS = static_cast<DistanceType>(0.00001);
            ElementType max_span = bbox[0].high - bbox[0].low;
            for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) {
                ElementType span = bbox[i].high - bbox[i].low;
                if (span > max_span) {
                    max_span = span;
                }
            }
            ElementType max_spread = -1;
            cutfeat = 0;
            for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
                ElementType span = bbox[i].high - bbox[i].low;
                if (span > (1 - EPS) * max_span) {
                    ElementType min_elem, max_elem;
                    computeMinMax(obj, ind, count, i, min_elem, max_elem);
                    ElementType spread = max_elem - min_elem;;
                    if (spread > max_spread) {
                        cutfeat = i;
                        max_spread = spread;
                    }
                }
            }
            // split in the middle
            DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
            ElementType min_elem, max_elem;
            computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);

            if (split_val < min_elem)
                cutval = min_elem;
            else if (split_val > max_elem)
                cutval = max_elem;
            else
                cutval = split_val;

            IndexType lim1, lim2;
            planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);

            if (lim1 > count / 2)
                index = lim1;
            else if (lim2 < count / 2)
                index = lim2;
            else
                index = count / 2;
        }

        /**
         *  Subdivide the list of points by a plane perpendicular on axe corresponding
         *  to the 'cutfeat' dimension at 'cutval' position.
         *
         *  On return:
         *  dataset[ind[0..lim1-1]][cutfeat]<cutval
         *  dataset[ind[lim1..lim2-1]][cutfeat]==cutval
         *  dataset[ind[lim2..count]][cutfeat]>cutval
         */
        void planeSplit(Derived &obj, IndexType *ind, const IndexType count,
                        int cutfeat, DistanceType &cutval, IndexType &lim1,
                        IndexType &lim2) {
            /* Move vector indices for left subtree to front of list. */
            IndexType left = 0;
            IndexType right = count - 1;
            for (;;) {
                while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval)
                    ++left;
                while (right && left <= right &&
                       dataset_get(obj, ind[right], cutfeat) >= cutval)
                    --right;
                if (left > right || !right)
                    break; // "!right" was added to support unsigned Index types
                std::swap(ind[left], ind[right]);
                ++left;
                --right;
            }
            /* If either list is empty, it means that all remaining features
             * are identical. Split in the middle to maintain a balanced tree.
             */
            lim1 = left;
            right = count - 1;
            for (;;) {
                while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval)
                    ++left;
                while (right && left <= right &&
                       dataset_get(obj, ind[right], cutfeat) > cutval)
                    --right;
                if (left > right || !right)
                    break; // "!right" was added to support unsigned Index types
                std::swap(ind[left], ind[right]);
                ++left;
                --right;
            }
            lim2 = left;
        }

        DistanceType computeInitialDistances(const Derived &obj,
                                             const ElementType *vec,
                                             distance_vector_t &dists) const {
            assert(vec);
            DistanceType distsq = DistanceType();

            for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
                if (vec[i] < obj.root_bbox[i].low) {
                    dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);
                    distsq += dists[i];
                }
                if (vec[i] > obj.root_bbox[i].high) {
                    dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);
                    distsq += dists[i];
                }
            }
            return distsq;
        }

        void save_tree(Derived &obj, FILE *stream, NodePtr tree) {
            save_value(stream, *tree);
            if (tree->child1 != NULL) {
                save_tree(obj, stream, tree->child1);
            }
            if (tree->child2 != NULL) {
                save_tree(obj, stream, tree->child2);
            }
        }

        void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {
            tree = obj.pool.template allocate<Node>();
            load_value(stream, *tree);
            if (tree->child1 != NULL) {
                load_tree(obj, stream, tree->child1);
            }
            if (tree->child2 != NULL) {
                load_tree(obj, stream, tree->child2);
            }
        }

        /**  Stores the index in a binary file.
         *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
         * loading the index object it must be constructed associated to the same
         * source of data points used while building it. See the example:
         * examples/saveload_example.cpp \sa loadIndex  */
        void saveIndex_(Derived &obj, FILE *stream) {
            save_value(stream, obj.m_size);
            save_value(stream, obj.dim);
            save_value(stream, obj.root_bbox);
            save_value(stream, obj.m_leaf_max_size);
            save_value(stream, obj.vind);
            save_tree(obj, stream, obj.root_node);
        }

        /**  Loads a previous index from a binary file.
         *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
         * index object must be constructed associated to the same source of data
         * points used while building the index. See the example:
         * examples/saveload_example.cpp \sa loadIndex  */
        void loadIndex_(Derived &obj, FILE *stream) {
            load_value(stream, obj.m_size);
            load_value(stream, obj.dim);
            load_value(stream, obj.root_bbox);
            load_value(stream, obj.m_leaf_max_size);
            load_value(stream, obj.vind);
            load_tree(obj, stream, obj.root_node);
        }
    };

/** @addtogroup kdtrees_grp KD-tree classes and adaptors
 * @{ */

/** kd-tree static index
 *
 * Contains the k-d trees and other information for indexing a set of points
 * for nearest-neighbor matching.
 *
 *  The class "DatasetAdaptor" must provide the following interface (can be
 * non-virtual, inlined methods):
 *
 *  \code
 *   // Must return the number of data poins
 *   inline size_t kdtree_get_point_count() const { ... }
 *
 *
 *   // Must return the dim'th component of the idx'th point in the class:
 *   inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
 *
 *   // Optional bounding-box computation: return false to default to a standard
 * bbox computation loop.
 *   //   Return true if the BBOX was already computed by the class and returned
 * in "bb" so it can be avoided to redo it again.
 *   //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
 * for point clouds) template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const
 *   {
 *      bb[0].low = ...; bb[0].high = ...;  // 0th dimension limits
 *      bb[1].low = ...; bb[1].high = ...;  // 1st dimension limits
 *      ...
 *      return true;
 *   }
 *
 *  \endcode
 *
 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
 * \tparam Distance The distance metric to use: nanoflann::metric_L1,
 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
 * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
 * be typically size_t or int
 */
    template<typename Distance, class DatasetAdaptor, int DIM = -1,
            typename IndexType = size_t>
    class KDTreeSingleIndexAdaptor
            : public KDTreeBaseClass<
                    KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>,
                    Distance, DatasetAdaptor, DIM, IndexType> {
    public:
        /** Deleted copy constructor*/
        KDTreeSingleIndexAdaptor(
                const KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>
                &) = delete;

        /**
         * The dataset used by this index
         */
        const DatasetAdaptor &dataset; //!< The source of our data

        const KDTreeSingleIndexAdaptorParams index_params;

        Distance distance;

        typedef typename nanoflann::KDTreeBaseClass<
                nanoflann::KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM,
                        IndexType>,
                Distance, DatasetAdaptor, DIM, IndexType>
                BaseClassRef;

        typedef typename BaseClassRef::ElementType ElementType;
        typedef typename BaseClassRef::DistanceType DistanceType;

        typedef typename BaseClassRef::Node Node;
        typedef Node *NodePtr;

        typedef typename BaseClassRef::Interval Interval;
        /** Define "BoundingBox" as a fixed-size or variable-size container depending
         * on "DIM" */
        typedef typename BaseClassRef::BoundingBox BoundingBox;

        /** Define "distance_vector_t" as a fixed-size or variable-size container
         * depending on "DIM" */
        typedef typename BaseClassRef::distance_vector_t distance_vector_t;

        /**
         * KDTree constructor
         *
         * Refer to docs in README.md or online in
         * https://github.com/jlblancoc/nanoflann
         *
         * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
         * for 3D points) is determined by means of:
         *  - The \a DIM template parameter if >0 (highest priority)
         *  - Otherwise, the \a dimensionality parameter of this constructor.
         *
         * @param inputData Dataset with the input features
         * @param params Basically, the maximum leaf node size
         */
        KDTreeSingleIndexAdaptor(const int dimensionality,
                                 const DatasetAdaptor &inputData,
                                 const KDTreeSingleIndexAdaptorParams &params =
                                 KDTreeSingleIndexAdaptorParams())
                : dataset(inputData), index_params(params), distance(inputData) {
            BaseClassRef::root_node = NULL;
            BaseClassRef::m_size = dataset.kdtree_get_point_count();
            BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
            BaseClassRef::dim = dimensionality;
            if (DIM > 0)
                BaseClassRef::dim = DIM;
            BaseClassRef::m_leaf_max_size = params.leaf_max_size;

            // Create a permutable array of indices to the input vectors.
            init_vind();
        }

        /**
         * Builds the index
         */
        void buildIndex() {
            BaseClassRef::m_size = dataset.kdtree_get_point_count();
            BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
            init_vind();
            this->freeIndex(*this);
            BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
            if (BaseClassRef::m_size == 0)
                return;
            computeBoundingBox(BaseClassRef::root_bbox);
            BaseClassRef::root_node =
                    this->divideTree(*this, 0, BaseClassRef::m_size,
                                     BaseClassRef::root_bbox); // construct the tree
        }

        /** \name Query methods
         * @{ */

        /**
         * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
         * inside the result object.
         *
         * Params:
         *     result = the result object in which the indices of the
         * nearest-neighbors are stored vec = the vector for which to search the
         * nearest neighbors
         *
         * \tparam RESULTSET Should be any ResultSet<DistanceType>
         * \return  True if the requested neighbors could be found.
         * \sa knnSearch, radiusSearch
         */
        template<typename RESULTSET>
        bool findNeighbors(RESULTSET &result, const ElementType *vec,
                           const SearchParams &searchParams) const {
            assert(vec);
            if (this->size(*this) == 0)
                return false;
            if (!BaseClassRef::root_node)
                throw std::runtime_error(
                        "[nanoflann] findNeighbors() called before building the index.");
            float epsError = 1 + searchParams.eps;

            distance_vector_t
                    dists; // fixed or variable-sized container (depending on DIM)
            auto zero = static_cast<decltype(result.worstDist())>(0);
            assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
                   zero); // Fill it with zeros.
            DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
            searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
                        epsError); // "count_leaf" parameter removed since was neither
            // used nor returned to the user.
            return result.full();
        }

        /**
         * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
         * Their indices are stored inside the result object. \sa radiusSearch,
         * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
         * with the original FLANN interface. \return Number `N` of valid points in
         * the result set. Only the first `N` entries in `out_indices` and
         * `out_distances_sq` will be valid. Return may be less than `num_closest`
         * only if the number of elements in the tree is less than `num_closest`.
         */
        size_t knnSearch(const ElementType *query_point, const size_t num_closest,
                         IndexType *out_indices, DistanceType *out_distances_sq,
                         const int /* nChecks_IGNORED */ = 10) const {
            nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
            resultSet.init(out_indices, out_distances_sq);
            this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
            return resultSet.size();
        }

        /**
         * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
         *  The output is given as a vector of pairs, of which the first element is a
         * point index and the second the corresponding distance. Previous contents of
         * \a IndicesDists are cleared.
         *
         *  If searchParams.sorted==true, the output list is sorted by ascending
         * distances.
         *
         *  For a better performance, it is advisable to do a .reserve() on the vector
         * if you have any wild guess about the number of expected matches.
         *
         *  \sa knnSearch, findNeighbors, radiusSearchCustomCallback
         * \return The number of points within the given radius (i.e. indices.size()
         * or dists.size() )
         */
        size_t
        radiusSearch(const ElementType *query_point, const DistanceType &radius,
                     std::vector<std::pair<IndexType, DistanceType>> &IndicesDists,
                     const SearchParams &searchParams) const {
            RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
            const size_t nFound =
                    radiusSearchCustomCallback(query_point, resultSet, searchParams);
            if (searchParams.sorted)
                std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
            return nFound;
        }

        /**
         * Just like radiusSearch() but with a custom callback class for each point
         * found in the radius of the query. See the source of RadiusResultSet<> as a
         * start point for your own classes. \sa radiusSearch
         */
        template<class SEARCH_CALLBACK>
        size_t radiusSearchCustomCallback(
                const ElementType *query_point, SEARCH_CALLBACK &resultSet,
                const SearchParams &searchParams = SearchParams()) const {
            this->findNeighbors(resultSet, query_point, searchParams);
            return resultSet.size();
        }

        /** @} */

    public:
        /** Make sure the auxiliary list \a vind has the same size than the current
         * dataset, and re-generate if size has changed. */
        void init_vind() {
            // Create a permutable array of indices to the input vectors.
            BaseClassRef::m_size = dataset.kdtree_get_point_count();
            if (BaseClassRef::vind.size() != BaseClassRef::m_size)
                BaseClassRef::vind.resize(BaseClassRef::m_size);
            for (size_t i = 0; i < BaseClassRef::m_size; i++)
                BaseClassRef::vind[i] = i;
        }

        void computeBoundingBox(BoundingBox &bbox) {
            resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
            if (dataset.kdtree_get_bbox(bbox)) {
                // Done! It was implemented in derived class
            } else {
                const size_t N = dataset.kdtree_get_point_count();
                if (!N)
                    throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
                                             "no data points found.");
                for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
                    bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i);
                }
                for (size_t k = 1; k < N; ++k) {
                    for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
                        if (this->dataset_get(*this, k, i) < bbox[i].low)
                            bbox[i].low = this->dataset_get(*this, k, i);
                        if (this->dataset_get(*this, k, i) > bbox[i].high)
                            bbox[i].high = this->dataset_get(*this, k, i);
                    }
                }
            }
        }

        /**
         * Performs an exact search in the tree starting from a node.
         * \tparam RESULTSET Should be any ResultSet<DistanceType>
         * \return true if the search should be continued, false if the results are
         * sufficient
         */
        template<class RESULTSET>
        bool searchLevel(RESULTSET &result_set, const ElementType *vec,
                         const NodePtr node, DistanceType mindistsq,
                         distance_vector_t &dists, const float epsError) const {
            /* If this is a leaf node, then do check and return. */
            if ((node->child1 == NULL) && (node->child2 == NULL)) {
                // count_leaf += (node->lr.right-node->lr.left);  // Removed since was
                // neither used nor returned to the user.
                DistanceType worst_dist = result_set.worstDist();
                for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
                     ++i) {
                    const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
                    DistanceType dist = distance.evalMetric(
                            vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
                    if (dist < worst_dist) {
                        if (!result_set.addPoint(dist, BaseClassRef::vind[i])) {
                            // the resultset doesn't want to receive any more points, we're done
                            // searching!
                            return false;
                        }
                    }
                }
                return true;
            }

            /* Which child branch should be taken first? */
            int idx = node->node_type.sub.divfeat;
            ElementType val = vec[idx];
            DistanceType diff1 = val - node->node_type.sub.divlow;
            DistanceType diff2 = val - node->node_type.sub.divhigh;

            NodePtr bestChild;
            NodePtr otherChild;
            DistanceType cut_dist;
            if ((diff1 + diff2) < 0) {
                bestChild = node->child1;
                otherChild = node->child2;
                cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
            } else {
                bestChild = node->child2;
                otherChild = node->child1;
                cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
            }

            /* Call recursively to search next level down. */
            if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) {
                // the resultset doesn't want to receive any more points, we're done
                // searching!
                return false;
            }

            DistanceType dst = dists[idx];
            mindistsq = mindistsq + cut_dist - dst;
            dists[idx] = cut_dist;
            if (mindistsq * epsError <= result_set.worstDist()) {
                if (!searchLevel(result_set, vec, otherChild, mindistsq, dists,
                                 epsError)) {
                    // the resultset doesn't want to receive any more points, we're done
                    // searching!
                    return false;
                }
            }
            dists[idx] = dst;
            return true;
        }

    public:
        /**  Stores the index in a binary file.
         *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
         * loading the index object it must be constructed associated to the same
         * source of data points used while building it. See the example:
         * examples/saveload_example.cpp \sa loadIndex  */
        void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }

        /**  Loads a previous index from a binary file.
         *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
         * index object must be constructed associated to the same source of data
         * points used while building the index. See the example:
         * examples/saveload_example.cpp \sa loadIndex  */
        void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }

    }; // class KDTree

/** kd-tree dynamic index
 *
 * Contains the k-d trees and other information for indexing a set of points
 * for nearest-neighbor matching.
 *
 *  The class "DatasetAdaptor" must provide the following interface (can be
 * non-virtual, inlined methods):
 *
 *  \code
 *   // Must return the number of data poins
 *   inline size_t kdtree_get_point_count() const { ... }
 *
 *   // Must return the dim'th component of the idx'th point in the class:
 *   inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
 *
 *   // Optional bounding-box computation: return false to default to a standard
 * bbox computation loop.
 *   //   Return true if the BBOX was already computed by the class and returned
 * in "bb" so it can be avoided to redo it again.
 *   //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
 * for point clouds) template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const
 *   {
 *      bb[0].low = ...; bb[0].high = ...;  // 0th dimension limits
 *      bb[1].low = ...; bb[1].high = ...;  // 1st dimension limits
 *      ...
 *      return true;
 *   }
 *
 *  \endcode
 *
 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
 * \tparam Distance The distance metric to use: nanoflann::metric_L1,
 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
 * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
 * be typically size_t or int
 */
    template<typename Distance, class DatasetAdaptor, int DIM = -1,
            typename IndexType = size_t>
    class KDTreeSingleIndexDynamicAdaptor_
            : public KDTreeBaseClass<KDTreeSingleIndexDynamicAdaptor_<
                    Distance, DatasetAdaptor, DIM, IndexType>,
                    Distance, DatasetAdaptor, DIM, IndexType> {
    public:
        /**
         * The dataset used by this index
         */
        const DatasetAdaptor &dataset; //!< The source of our data

        KDTreeSingleIndexAdaptorParams index_params;

        std::vector<int> &treeIndex;

        Distance distance;

        typedef typename nanoflann::KDTreeBaseClass<
                nanoflann::KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM,
                        IndexType>,
                Distance, DatasetAdaptor, DIM, IndexType>
                BaseClassRef;

        typedef typename BaseClassRef::ElementType ElementType;
        typedef typename BaseClassRef::DistanceType DistanceType;

        typedef typename BaseClassRef::Node Node;
        typedef Node *NodePtr;

        typedef typename BaseClassRef::Interval Interval;
        /** Define "BoundingBox" as a fixed-size or variable-size container depending
         * on "DIM" */
        typedef typename BaseClassRef::BoundingBox BoundingBox;

        /** Define "distance_vector_t" as a fixed-size or variable-size container
         * depending on "DIM" */
        typedef typename BaseClassRef::distance_vector_t distance_vector_t;

        /**
         * KDTree constructor
         *
         * Refer to docs in README.md or online in
         * https://github.com/jlblancoc/nanoflann
         *
         * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
         * for 3D points) is determined by means of:
         *  - The \a DIM template parameter if >0 (highest priority)
         *  - Otherwise, the \a dimensionality parameter of this constructor.
         *
         * @param inputData Dataset with the input features
         * @param params Basically, the maximum leaf node size
         */
        KDTreeSingleIndexDynamicAdaptor_(
                const int dimensionality, const DatasetAdaptor &inputData,
                std::vector<int> &treeIndex_,
                const KDTreeSingleIndexAdaptorParams &params =
                KDTreeSingleIndexAdaptorParams())
                : dataset(inputData), index_params(params), treeIndex(treeIndex_),
                  distance(inputData) {
            BaseClassRef::root_node = NULL;
            BaseClassRef::m_size = 0;
            BaseClassRef::m_size_at_index_build = 0;
            BaseClassRef::dim = dimensionality;
            if (DIM > 0)
                BaseClassRef::dim = DIM;
            BaseClassRef::m_leaf_max_size = params.leaf_max_size;
        }

        /** Assignment operator definiton */
        KDTreeSingleIndexDynamicAdaptor_
        operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) {
            KDTreeSingleIndexDynamicAdaptor_ tmp(rhs);
            std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind);
            std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);
            std::swap(index_params, tmp.index_params);
            std::swap(treeIndex, tmp.treeIndex);
            std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);
            std::swap(BaseClassRef::m_size_at_index_build,
                      tmp.BaseClassRef::m_size_at_index_build);
            std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);
            std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);
            std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);
            return *this;
        }

        /**
         * Builds the index
         */
        void buildIndex() {
            BaseClassRef::m_size = BaseClassRef::vind.size();
            this->freeIndex(*this);
            BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
            if (BaseClassRef::m_size == 0)
                return;
            computeBoundingBox(BaseClassRef::root_bbox);
            BaseClassRef::root_node =
                    this->divideTree(*this, 0, BaseClassRef::m_size,
                                     BaseClassRef::root_bbox); // construct the tree
        }

        /** \name Query methods
         * @{ */

        /**
         * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
         * inside the result object.
         *
         * Params:
         *     result = the result object in which the indices of the
         * nearest-neighbors are stored vec = the vector for which to search the
         * nearest neighbors
         *
         * \tparam RESULTSET Should be any ResultSet<DistanceType>
         * \return  True if the requested neighbors could be found.
         * \sa knnSearch, radiusSearch
         */
        template<typename RESULTSET>
        bool findNeighbors(RESULTSET &result, const ElementType *vec,
                           const SearchParams &searchParams) const {
            assert(vec);
            if (this->size(*this) == 0)
                return false;
            if (!BaseClassRef::root_node)
                return false;
            float epsError = 1 + searchParams.eps;

            // fixed or variable-sized container (depending on DIM)
            distance_vector_t dists;
            // Fill it with zeros.
            assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
                   static_cast<typename distance_vector_t::value_type>(0));
            DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
            searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
                        epsError); // "count_leaf" parameter removed since was neither
            // used nor returned to the user.
            return result.full();
        }

        /**
         * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
         * Their indices are stored inside the result object. \sa radiusSearch,
         * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
         * with the original FLANN interface. \return Number `N` of valid points in
         * the result set. Only the first `N` entries in `out_indices` and
         * `out_distances_sq` will be valid. Return may be less than `num_closest`
         * only if the number of elements in the tree is less than `num_closest`.
         */
        size_t knnSearch(const ElementType *query_point, const size_t num_closest,
                         IndexType *out_indices, DistanceType *out_distances_sq,
                         const int /* nChecks_IGNORED */ = 10) const {
            nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
            resultSet.init(out_indices, out_distances_sq);
            this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
            return resultSet.size();
        }

        /**
         * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
         *  The output is given as a vector of pairs, of which the first element is a
         * point index and the second the corresponding distance. Previous contents of
         * \a IndicesDists are cleared.
         *
         *  If searchParams.sorted==true, the output list is sorted by ascending
         * distances.
         *
         *  For a better performance, it is advisable to do a .reserve() on the vector
         * if you have any wild guess about the number of expected matches.
         *
         *  \sa knnSearch, findNeighbors, radiusSearchCustomCallback
         * \return The number of points within the given radius (i.e. indices.size()
         * or dists.size() )
         */
        size_t
        radiusSearch(const ElementType *query_point, const DistanceType &radius,
                     std::vector<std::pair<IndexType, DistanceType>> &IndicesDists,
                     const SearchParams &searchParams) const {
            RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
            const size_t nFound =
                    radiusSearchCustomCallback(query_point, resultSet, searchParams);
            if (searchParams.sorted)
                std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
            return nFound;
        }

        /**
         * Just like radiusSearch() but with a custom callback class for each point
         * found in the radius of the query. See the source of RadiusResultSet<> as a
         * start point for your own classes. \sa radiusSearch
         */
        template<class SEARCH_CALLBACK>
        size_t radiusSearchCustomCallback(
                const ElementType *query_point, SEARCH_CALLBACK &resultSet,
                const SearchParams &searchParams = SearchParams()) const {
            this->findNeighbors(resultSet, query_point, searchParams);
            return resultSet.size();
        }

        /** @} */

    public:
        void computeBoundingBox(BoundingBox &bbox) {
            resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));

            if (dataset.kdtree_get_bbox(bbox)) {
                // Done! It was implemented in derived class
            } else {
                const size_t N = BaseClassRef::m_size;
                if (!N)
                    throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
                                             "no data points found.");
                for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
                    bbox[i].low = bbox[i].high =
                            this->dataset_get(*this, BaseClassRef::vind[0], i);
                }
                for (size_t k = 1; k < N; ++k) {
                    for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
                        if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low)
                            bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i);
                        if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high)
                            bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i);
                    }
                }
            }
        }

        /**
         * Performs an exact search in the tree starting from a node.
         * \tparam RESULTSET Should be any ResultSet<DistanceType>
         */
        template<class RESULTSET>
        void searchLevel(RESULTSET &result_set, const ElementType *vec,
                         const NodePtr node, DistanceType mindistsq,
                         distance_vector_t &dists, const float epsError) const {
            /* If this is a leaf node, then do check and return. */
            if ((node->child1 == NULL) && (node->child2 == NULL)) {
                // count_leaf += (node->lr.right-node->lr.left);  // Removed since was
                // neither used nor returned to the user.
                DistanceType worst_dist = result_set.worstDist();
                for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
                     ++i) {
                    const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
                    if (treeIndex[index] == -1)
                        continue;
                    DistanceType dist = distance.evalMetric(
                            vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
                    if (dist < worst_dist) {
                        if (!result_set.addPoint(
                                static_cast<typename RESULTSET::DistanceType>(dist),
                                static_cast<typename RESULTSET::IndexType>(
                                        BaseClassRef::vind[i]))) {
                            // the resultset doesn't want to receive any more points, we're done
                            // searching!
                            return; // false;
                        }
                    }
                }
                return;
            }

            /* Which child branch should be taken first? */
            int idx = node->node_type.sub.divfeat;
            ElementType val = vec[idx];
            DistanceType diff1 = val - node->node_type.sub.divlow;
            DistanceType diff2 = val - node->node_type.sub.divhigh;

            NodePtr bestChild;
            NodePtr otherChild;
            DistanceType cut_dist;
            if ((diff1 + diff2) < 0) {
                bestChild = node->child1;
                otherChild = node->child2;
                cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
            } else {
                bestChild = node->child2;
                otherChild = node->child1;
                cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
            }

            /* Call recursively to search next level down. */
            searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);

            DistanceType dst = dists[idx];
            mindistsq = mindistsq + cut_dist - dst;
            dists[idx] = cut_dist;
            if (mindistsq * epsError <= result_set.worstDist()) {
                searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
            }
            dists[idx] = dst;
        }

    public:
        /**  Stores the index in a binary file.
         *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
         * loading the index object it must be constructed associated to the same
         * source of data points used while building it. See the example:
         * examples/saveload_example.cpp \sa loadIndex  */
        void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }

        /**  Loads a previous index from a binary file.
         *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
         * index object must be constructed associated to the same source of data
         * points used while building the index. See the example:
         * examples/saveload_example.cpp \sa loadIndex  */
        void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
    };

/** kd-tree dynaimic index
 *
 * class to create multiple static index and merge their results to behave as
 * single dynamic index as proposed in Logarithmic Approach.
 *
 *  Example of usage:
 *  examples/dynamic_pointcloud_example.cpp
 *
 * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
 * \tparam Distance The distance metric to use: nanoflann::metric_L1,
 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
 * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
 * be typically size_t or int
 */
    template<typename Distance, class DatasetAdaptor, int DIM = -1,
            typename IndexType = size_t>
    class KDTreeSingleIndexDynamicAdaptor {
    public:
        typedef typename Distance::ElementType ElementType;
        typedef typename Distance::DistanceType DistanceType;

    protected:
        size_t m_leaf_max_size;
        size_t treeCount;
        size_t pointCount;

        /**
         * The dataset used by this index
         */
        const DatasetAdaptor &dataset; //!< The source of our data

        std::vector<int> treeIndex; //!< treeIndex[idx] is the index of tree in which
        //!< point at idx is stored. treeIndex[idx]=-1
        //!< means that point has been removed.

        KDTreeSingleIndexAdaptorParams index_params;

        int dim; //!< Dimensionality of each data point

        typedef KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>
                index_container_t;
        std::vector<index_container_t> index;

    public:
        /** Get a const ref to the internal list of indices; the number of indices is
         * adapted dynamically as the dataset grows in size. */
        const std::vector<index_container_t> &getAllIndices() const { return index; }

    private:
        /** finds position of least significant unset bit */
        int First0Bit(IndexType num) {
            int pos = 0;
            while (num & 1) {
                num = num >> 1;
                pos++;
            }
            return pos;
        }

        /** Creates multiple empty trees to handle dynamic support */
        void init() {
            typedef KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>
                    my_kd_tree_t;
            std::vector<my_kd_tree_t> index_(
                    treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params));
            index = index_;
        }

    public:
        Distance distance;

        /**
         * KDTree constructor
         *
         * Refer to docs in README.md or online in
         * https://github.com/jlblancoc/nanoflann
         *
         * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
         * for 3D points) is determined by means of:
         *  - The \a DIM template parameter if >0 (highest priority)
         *  - Otherwise, the \a dimensionality parameter of this constructor.
         *
         * @param inputData Dataset with the input features
         * @param params Basically, the maximum leaf node size
         */
        KDTreeSingleIndexDynamicAdaptor(const int dimensionality,
                                        const DatasetAdaptor &inputData,
                                        const KDTreeSingleIndexAdaptorParams &params =
                                        KDTreeSingleIndexAdaptorParams(),
                                        const size_t maximumPointCount = 1000000000U)
                : dataset(inputData), index_params(params), distance(inputData) {
            treeCount = static_cast<size_t>(std::log2(maximumPointCount));
            pointCount = 0U;
            dim = dimensionality;
            treeIndex.clear();
            if (DIM > 0)
                dim = DIM;
            m_leaf_max_size = params.leaf_max_size;
            init();
            const size_t num_initial_points = dataset.kdtree_get_point_count();
            if (num_initial_points > 0) {
                addPoints(0, num_initial_points - 1);
            }
        }

        /** Deleted copy constructor*/
        KDTreeSingleIndexDynamicAdaptor(
                const KDTreeSingleIndexDynamicAdaptor<Distance, DatasetAdaptor, DIM,
                        IndexType> &) = delete;

        /** Add points to the set, Inserts all points from [start, end] */
        void addPoints(IndexType start, IndexType end) {
            size_t count = end - start + 1;
            treeIndex.resize(treeIndex.size() + count);
            for (IndexType idx = start; idx <= end; idx++) {
                int pos = First0Bit(pointCount);
                index[pos].vind.clear();
                treeIndex[pointCount] = pos;
                for (int i = 0; i < pos; i++) {
                    for (int j = 0; j < static_cast<int>(index[i].vind.size()); j++) {
                        index[pos].vind.push_back(index[i].vind[j]);
                        if (treeIndex[index[i].vind[j]] != -1)
                            treeIndex[index[i].vind[j]] = pos;
                    }
                    index[i].vind.clear();
                    index[i].freeIndex(index[i]);
                }
                index[pos].vind.push_back(idx);
                index[pos].buildIndex();
                pointCount++;
            }
        }

        /** Remove a point from the set (Lazy Deletion) */
        void removePoint(size_t idx) {
            if (idx >= pointCount)
                return;
            treeIndex[idx] = -1;
        }

        /**
         * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
         * inside the result object.
         *
         * Params:
         *     result = the result object in which the indices of the
         * nearest-neighbors are stored vec = the vector for which to search the
         * nearest neighbors
         *
         * \tparam RESULTSET Should be any ResultSet<DistanceType>
         * \return  True if the requested neighbors could be found.
         * \sa knnSearch, radiusSearch
         */
        template<typename RESULTSET>
        bool findNeighbors(RESULTSET &result, const ElementType *vec,
                           const SearchParams &searchParams) const {
            for (size_t i = 0; i < treeCount; i++) {
                index[i].findNeighbors(result, &vec[0], searchParams);
            }
            return result.full();
        }
    };

/** An L2-metric KD-tree adaptor for working with data directly stored in an
 * Eigen Matrix, without duplicating the data storage. Each row in the matrix
 * represents a point in the state space.
 *
 *  Example of usage:
 * \code
 * 	Eigen::Matrix<num_t,Dynamic,Dynamic>  mat;
 * 	// Fill out "mat"...
 *
 * 	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> >
 * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t   mat_index(mat, max_leaf
 * ); mat_index.index->buildIndex(); mat_index.index->... \endcode
 *
 *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality
 * for the points in the data set, allowing more compiler optimizations. \tparam
 * Distance The distance metric to use: nanoflann::metric_L1,
 * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
 */
    template<class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2>
    struct KDTreeEigenMatrixAdaptor {
        typedef KDTreeEigenMatrixAdaptor<MatrixType, DIM, Distance> self_t;
        typedef typename MatrixType::Scalar num_t;
        typedef typename MatrixType::Index IndexType;
        typedef
        typename Distance::template traits<num_t, self_t>::distance_t metric_t;
        typedef KDTreeSingleIndexAdaptor<metric_t, self_t,
                MatrixType::ColsAtCompileTime, IndexType>
                index_t;

        index_t *index; //! The kd-tree index for the user to call its methods as
        //! usual with any other FLANN index.

        /// Constructor: takes a const ref to the matrix object with the data points
        KDTreeEigenMatrixAdaptor(const size_t dimensionality,
                                 const std::reference_wrapper<const MatrixType> &mat,
                                 const int leaf_max_size = 10)
                : m_data_matrix(mat) {
            const auto dims = mat.get().cols();
            if (size_t(dims) != dimensionality)
                throw std::runtime_error(
                        "Error: 'dimensionality' must match column count in data matrix");
            if (DIM > 0 && int(dims) != DIM)
                throw std::runtime_error(
                        "Data set dimensionality does not match the 'DIM' template argument");
            index =
                    new index_t(static_cast<int>(dims), *this /* adaptor */,
                                nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
            index->buildIndex();
        }

    public:
        /** Deleted copy constructor */
        KDTreeEigenMatrixAdaptor(const self_t &) = delete;

        ~KDTreeEigenMatrixAdaptor() { delete index; }

        const std::reference_wrapper<const MatrixType> m_data_matrix;

        /** Query for the \a num_closest closest points to a given point (entered as
         * query_point[0:dim-1]). Note that this is a short-cut method for
         * index->findNeighbors(). The user can also call index->... methods as
         * desired. \note nChecks_IGNORED is ignored but kept for compatibility with
         * the original FLANN interface.
         */
        inline void query(const num_t *query_point, const size_t num_closest,
                          IndexType *out_indices, num_t *out_distances_sq,
                          const int /* nChecks_IGNORED */ = 10) const {
            nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
            resultSet.init(out_indices, out_distances_sq);
            index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
        }

        /** @name Interface expected by KDTreeSingleIndexAdaptor
         * @{ */

        const self_t &derived() const { return *this; }

        self_t &derived() { return *this; }

        // Must return the number of data points
        inline size_t kdtree_get_point_count() const {
            return m_data_matrix.get().rows();
        }

        // Returns the dim'th component of the idx'th point in the class:
        inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {
            return m_data_matrix.get().coeff(idx, IndexType(dim));
        }

        // Optional bounding-box computation: return false to default to a standard
        // bbox computation loop.
        //   Return true if the BBOX was already computed by the class and returned in
        //   "bb" so it can be avoided to redo it again. Look at bb.size() to find out
        //   the expected dimensionality (e.g. 2 or 3 for point clouds)
        template<class BBOX>
        bool kdtree_get_bbox(BBOX & /*bb*/) const {
            return false;
        }

        /** @} */

    }; // end of KDTreeEigenMatrixAdaptor
    /** @} */

/** @} */ // end of grouping
} // namespace nanoflann

#endif /* NANOFLANN_HPP_ */
